#pragma once
#include <list>
#include "axisgroup/axisgroup_robot.h"
#include "motion/adapt_notch.h"

class JAKADynIntf;

class CollisionDetection
{
public:
    CollisionDetection(std::shared_ptr<JAKADynIntf> dyna);
    CollisionDetection() = delete;
    virtual ~CollisionDetection() = default;
    typedef enum CollisionState
    {
        NO_ALARM = 0,
        GM_ALARM_JOINT1 = 1,  // 动量碰撞报警
        GM_ALARM_JOINT2 = 2,
        GM_ALARM_JOINT3 = 3,
        GM_ALARM_JOINT4 = 4,
        GM_ALARM_JOINT5 = 5,
        GM_ALARM_JOINT6 = 6,
        MOMENTUM_ALARM = 7,  // 动量超限
        POWER_ALARM = 8      // 功率超限
    } SaftyMonitorRes;

public:
    void set_end_force_limit(double limit);  // 0~100
    void set_joint_sensitivity(const RobJointData<double>& joint_sensitivity);
    void set_GM_factor(const double* factor);

    SaftyMonitorRes SaftyAction(const axisgroup::Robot::MovingState& state);

private:
    std::shared_ptr<JAKADynIntf> dyn_;

    const static int MAX_DETECT_COUNT = 3;
    int collision_count = 0;

    struct LowPassFilter
    {
        double last_x = 0;
        double last_y = 0;
        void reset();
        double do_filter(double cutFrq, double input);
    };
    RobJointData<LowPassFilter> low_pass_filter_;
    RobJointData<NotchFilter> notch_filter1_;
    RobJointData<NotchFilter> notch_filter2_;

    bool allow_alarm_ = true;
    bool has_wrong_vel = false;
    double end_force_limit_ = 100;  // APP 设置上的碰撞灵敏度 (目前时0～100%)
    RobJointData<double> joint_sensitivity_;

    double GM_factor_[7] = {10000.0, 1, 1.2, 1.4, 1.6, 2.0, 10000.0};  // 后门参数，不暴露给用户
    int last_collision_sensity_level = 0;
    int count_change_level = 0;
    const static int LEVEL_DOWN_DELAY = 5;

    const static int MAX_ACCU_NUM = 15;
    std::list<double> jerk_acuumulator_[RobJointData<double>::MAX_AXIS];

    RobJointData<double> last_joint_sensitivity;
    RobJointData<double> joint_sensitivity_filted;

    int collisionDetectionOnMomentumObserver(const axisgroup::Robot::MovingState& state, const double* toolParameters);
    void GMGetBaseSensitivityFromApp(double tool_mass,
                                     RobJointData<double>& high_jerk_compensate_factor,
                                     RobJointData<double>& joint_basic_sensitivity,
                                     RobJointData<double>& high_speed_compensate_factor,
                                     RobJointData<double>& low_speed_compensate_factor,
                                     RobJointData<double>& high_acc_compensate_factor,
                                     RobJointData<double>& tool_compensate_fator);
    void GMGetJointJerkCompensate(const RobJointData<double>& joint_jerk,
                                  const double* toolParameters,
                                  const RobJointData<double>& high_jerk_compensate_factor,
                                  RobJointData<double>& joint_jerk_compensate);
    void GMGetCollisionSensitivity(const RobJointData<double>& jntPosCmd,
                                   const RobJointData<double>& jntVelFb,
                                   const RobJointData<double>& jntVelCmd,
                                   const RobJointData<double>& jntAccCmd,
                                   const RobJointData<double>& jntJerkCmd,
                                   const double* toolParameters,
                                   RobJointData<double>& joint_sensitivity);

private:
    bool _gm_record_enable = 0;
    FILE* _gm_debug_file = NULL;
    bool gm_hist_record_enable = 0;
    FILE* gm_hist_record_file = NULL;
    const static int HISTORY_RECORD = 250;
    int history_gm_record_count = 0;
    void do_record(const axisgroup::Robot::MovingState& state,
                   const RobJointData<double>& joint_sensitivity,
                   const RobJointData<double>& torpEst,
                   const RobJointData<double>& torpEst_N,
                   const RobJointData<double>& torpEst_N1,
                   const RobJointData<double>& torpEst_F,
                   int collision_count,
                   int collision_flag);
    void enableGmRecord(int enable);
    void GMHistoryRecord(const axisgroup::Robot::MovingState& state,
                         const RobJointData<double>& torpEst,
                         const RobJointData<double>& joint_sensitivity,
                         int collision_count,
                         int collision_flag);

    struct HishtoryData
    {
        RobJointData<double> history_torEst_value;
        RobJointData<double> history_tor_value;
        RobJointData<double> history_sensitivity_value;
        RobJointData<double> history_q_value;
        RobJointData<double> history_q_cmd_value;
        RobJointData<double> history_qd_value;
        RobJointData<double> history_jointTorqFb_value;
        RobJointData<double> history_jointTorqCmd_value;
        double history_linear_vel_value;
        int history_collision_count_value;
        int history_collision_flag_value;
    };
    std::list<HishtoryData> hist_data_;
};
